/**
 * @file euler_kmeans++.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief 基于欧拉核的 K-means++ 算法
 * @version 0.1
 * @date 2020-07-28
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "euler_kmeans++.h"

euler_kmeans_plus2::euler_kmeans_plus2(/* args */)
{
}

euler_kmeans_plus2::~euler_kmeans_plus2()
{
}

double euler_kmeans_plus2::DistanceEuler(pcl::PointXYZ param1, pcl::PointXYZ param2)
{
  return (2 
          - cosf((param1.x - param2.x) * alpha * PI) 
          - cosf((param1.y - param2.y) * alpha * PI));
}

double euler_kmeans_plus2::DistanceEuler(pcl::PointXYZ param1, POINT_COMPLEX param2)
{
  POINT_COMPLEX tmp;
  tmp = PointXYZTOComplex(param1);
  return (2 
          - tmp.x.real * param2.x.real - tmp.x.imaginary * param2.x.imaginary 
          - tmp.y.real * param2.y.real - tmp.y.imaginary * param2.y.imaginary);
}

POINT_COMPLEX euler_kmeans_plus2::PointXYZTOComplex(pcl::PointXYZ param)
{
  POINT_COMPLEX tmp;

  tmp.x.real = cosf(param.x * alpha * PI);      //  * sqrt2reciprocal;
  tmp.x.imaginary = sinf(param.x * alpha * PI); //  * sqrt2reciprocal;
  tmp.y.real = cosf(param.y * alpha * PI);      //  * sqrt2reciprocal;
  tmp.y.imaginary = sinf(param.y * alpha * PI); //  * sqrt2reciprocal;

  return tmp;
}

void euler_kmeans_plus2::SetPointCloudIn(pcl::PointCloud<pcl::PointXYZ>::ConstPtr PointCloudConstPtr)
{
  pointcloud_in = PointCloudConstPtr;
  ROS_INFO("pointcloud_in size is %d", pointcloud_in->size());
}

void euler_kmeans_plus2::PonitCloudNormalization(void)
{
  pcl::PointCloud<pcl::PointXYZ> tmp;
  pcl::PointXYZ point;
  for (uint16_t i = 0; i < pointcloud_in->size(); i++)
  {
    pcl::PointXYZ point;
    point.x = pointcloud_in->points[i].x;
    point.y = pointcloud_in->points[i].y;
    if ((double)point.y < 20 && (double)point.y > -20)
    {
      if ((double)point.x < 20 && (double)point.x > -20)
      {
        point.x = pointcloud_in->points[i].x / (double)20;
        point.y = pointcloud_in->points[i].y / (double)20;
        point.z = 0;
        tmp.points.push_back(point);
      }
    }
  }
  pointcloud_in = tmp.makeShared();
}

void euler_kmeans_plus2::InitKClusterCenter(void)
{
  if (initial_point.empty())
  {
    #define random(x) (rand()%x)
    srand((int32_t)(ros::Time::now().toSec()));
    initial_point.push_back(PointXYZTOComplex(pointcloud_in->points[random(pointcloud_in->size())]));

    for (uint16_t i = 1; i < K; i++)
    {
      // 计算每个点与最近聚类中心的距离
      double sum_distance = 0;
      double distance, temp;

      for (uint16_t j = 0; j < pointcloud_in->size(); j++)
      {
        // 计算出距离当前点最小的聚类中心
        distance = 2;
        temp = 0;
        for (uint16_t k = 0; k < initial_point.size(); k++)
        {
          temp = DistanceEuler(pointcloud_in->points[j], initial_point[k]);
          distance = distance<temp?distance:temp;
        }
        sum_distance += distance;
        probability.push_back(sum_distance);
      }
      // 通过伪随机数归一化给出下一个聚类中心
      double next = (double)(random((uint16_t)sum_distance));
      for (uint16_t i = 0; i < probability.size(); i++)
      {
        if (next <= probability[i])
        {
          initial_point.push_back(PointXYZTOComplex(pointcloud_in->points[i]));
          probability.clear();
          break;
        }
      }
    }
  }
  
}

void euler_kmeans_plus2::ClusterCenterIteration(void)
{
  POINTCLOUDINDICES tmp;
  tmp.euler_mean_center.x.imaginary = 0;
  tmp.euler_mean_center.x.real = 0;
  tmp.euler_mean_center.y.imaginary = 0;
  tmp.euler_mean_center.y.real = 0;
  for (uint8_t i = 0; i < K; i++)
  {
    cluster_indices.push_back(tmp);
  }

  double distance_euler, distance_tmep;
  uint16_t point_filter;
  POINT_COMPLEX iteration_center;
  POINT_COMPLEX temp_euler_point;
  uint8_t iteration_num = 0;

  while ((iteration_num < ITERATIONS_MAX) )//&& )
  {
    // 将点云按照欧拉距离分类
    for (uint16_t i = 0; i < pointcloud_in->size(); i++)
    {
      distance_euler = 2;
      distance_tmep = 0;
      point_filter = 0;
      for (uint16_t j = 0; j < initial_point.size(); j++)
      {
        distance_tmep = DistanceEuler(pointcloud_in->points[i],initial_point[j]);
        if (distance_euler > distance_tmep)
        {
          distance_euler = distance_tmep;
          point_filter = j;
        }
      }
      cluster_indices[point_filter].indices.push_back(i);
    }

    // 计算点云团新中心
    initial_point.clear();
    for (uint i = 0; i < cluster_indices.size(); i++)
    {
      for (uint16_t j = 0; j < cluster_indices[i].indices.size(); j++)
      {
        temp_euler_point = PointXYZTOComplex(pointcloud_in->points[cluster_indices[i].indices[j]]);
        // initial_point.push_back(temp_euler_point);

        iteration_center.x.real += temp_euler_point.x.real/(double)cluster_indices[i].indices.size();
        iteration_center.x.imaginary += temp_euler_point.x.imaginary/(double)cluster_indices[i].indices.size();
        iteration_center.y.real += temp_euler_point.y.real/(double)cluster_indices[i].indices.size();
        iteration_center.y.imaginary += temp_euler_point.y.imaginary/(double)cluster_indices[i].indices.size();
      }
      double indices_num  = (double)cluster_indices[i].indices.size();
      iteration_center.x.real /= indices_num;
      iteration_center.x.imaginary /= indices_num;
      iteration_center.y.real /= indices_num;
      iteration_center.y.imaginary /= indices_num;
      initial_point.push_back(iteration_center);
      // ROS_INFO( "iteration_center[%d] is %f, %f, %f ,%f", i, iteration_center.x.real, iteration_center.x.imaginary, iteration_center.y.real, iteration_center.y.imaginary);
      iteration_center.x.real = 0;
      iteration_center.x.imaginary = 0;
      iteration_center.y.real = 0;
      iteration_center.y.imaginary = 0;
    }
    iteration_num ++;
  }

  pcl::PointXYZRGB point_rgb;
  pcl::PointCloud<pcl::PointXYZRGB> point_cloud_cluster_out;

  for (uint16_t i = 0; i < cluster_indices.size(); i++)
  {
    #define random(x) (rand()%x)
    uint8_t r(random(255)), g(random(255)), b(random(255));
    for (uint16_t j = 0; j < cluster_indices[i].indices.size(); j++)
    {
      point_rgb.x = pointcloud_in->points[cluster_indices[i].indices[j]].x;
      point_rgb.y = pointcloud_in->points[cluster_indices[i].indices[j]].y;
      point_rgb.z = pointcloud_in->points[cluster_indices[i].indices[j]].z;
      point_rgb.rgb = (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
      point_cloud_cluster_out.points.push_back(point_rgb);
    }
  }
  pcl::toROSMsg(point_cloud_cluster_out, point_cloud_out);
  point_cloud_out.header.frame_id = "/car";
  pub.publish(point_cloud_out);

}